from pyrobot.brain import *
import pyrobot.system.share as share
from pyrobot.map.localview import LocalView
from pyrobot.map.globalview import GlobalView
from pyrobot.algorithms.pso import PSO
from pyrobot.bean.map.point import Point
import thread
import random
import math
from math import cos, sin, pi, sqrt

class MapBuilder(Brain):
   def setup(self):
      # We want our map to measure in MM, so we first store our current unit of measure
      units = self.robot.range.units
      # We then reset our measurements to MMs
      self.robot.range.units = 'MM'
      # Calculate the maximum range of our sensors
      rangeMaxMM = self.robot.range.getMaxvalue()
      sizeMM = rangeMaxMM * 3 + (self.robot.radius / 1000.0) # in MMs
      # Reset our unit of measure
      self.robot.range.units = units
      # Now, we create our Local Perceptual Space window - this will hold our local map
      # Map will be 20px by 20px and will represent a height and width of sizeMM (total sensor range)
      self.lps = LocalView( 200, 200,
                      widthMM = sizeMM * 3,
                      heightMM = sizeMM * 3)
      # Then create our Global Perceptual Space window - this will hold our global map
      # This map will be 500px by 500px and will represent an area N times the size of our maximum range
      self.gps = GlobalView( cols=400, rows=400,
                      heightMM = sizeMM * 6, widthMM = sizeMM * 6)
      #self.joystick = Joystick(share.gui)
      self.need_redraw = False
      self.lock = thread.allocate_lock()
      self.RotateNeed = 0   #2 means it needs rotation, 1 means it is rotating, 0 means it does not need rotate currently
      self.prev_robotth = 0
      self.holecoords = []
      # action control
      # current status of robot
      self.status = "SENSE"  
      self.directMove = False
      self.offsetDist = 0.0   
      #
#      self.nextBestView = Point(0, 0)
      #
      self.freeEdgesFont = []
      self.freeEdgesBack = []
      self.globalFreeEdges = []
      self.bFreeFont = False
      self.bFreeBack = False
      # turn around
      self.robotPrevTheta = None
      
      # trick
      self.positionList = []
      point1 = Point(1500, -500)
      point2 = Point(400, -3000)
      point3 = Point(-200, -4000)
      self.positionList.append(point1)
      self.positionList.append(point2)
      self.positionList.append(point3)
      self.posIndex = 0
      
 
   def __getHits__(self, item, index):
     originalUnits = self.robot.__dict__[item].units
     self.robot.__dict__[item].units = 'METERS'
     offx, offy, z, theta, arc = self.robot.__dict__[item][index].geometry
     dist = self.robot.__dict__[item][index].value
     # local coordination, convert to MMs         
     robotTheta = self.robot.th * math.pi / 180
     hitx_global_angle = cos(theta+robotTheta) * dist * 1000 + offx
     hity_global_angle = sin(theta+robotTheta) * dist * 1000 + offy   
     self.robot.__dict__[item].units = originalUnits     
     
     return (hitx_global_angle+self.robot.x*1000, hity_global_angle+self.robot.y*1000)
 
   def __calculateTurnAngle__(self, x2, y2, x3, y3):
       print "__calculateTurnAngle__"
#       currentPoint = Point(self.robot.x*1000, self.robot.y*1000)
       p1 = Point(x2, y2)
       p2 = Point(x3, y3)
       theta = p1.slopeAngle(p2)
       
       print "theta = p1.slopeAngle(p2) ", theta
       
       robotTheta = self.robot.th
       if self.robot.th > 180:
           robotTheta = self.robot.th - 360
       else:
           robotTheta = self.robot.th
       if theta > 180:
           theta = theta - 360
       else:
           theta = theta
       # trim theta
       if math.fabs(math.fabs(theta)-90) < 45:
#           theta = theta / math.fabs(theta) * 90
           theta = robotTheta / math.fabs(robotTheta) * 90
       else:
           if math.fabs(robotTheta)<90:
               theta = 0
           elif robotTheta < 0: 
               theta = -180
           elif robotTheta > 0:
               theta = 180
       #
       turnPower = 0
       if math.fabs(theta-robotTheta) > 6:
           print "theta ", theta
           print "robotTheta ", robotTheta
           print "(theta-robotTheta) ", (theta-robotTheta)
           
           if math.fabs(theta - robotTheta) > 180:
               if theta < 0 and robotTheta > 0:
                   theta = 360 + theta
               elif theta > 0 and robotTheta < 0:
                   robotTheta = 360 + robotTheta 
           turningDir = (theta - robotTheta) / math.fabs(theta - robotTheta)
           
           if math.fabs(theta - robotTheta) > 15:
               turnPower = turningDir * 0.2
           elif math.fabs(theta - robotTheta) > 9:
               turnPower = turningDir * 0.15
           else:
               turnPower = turningDir * 0.05                   
           print "turnPower ", turnPower
           return turnPower  
       else:
           print "turnPower ", turnPower
           return 0  
 
   def __moveOrNot__(self, dist, position):
        front = min([s.distance() for s in self.robot.range["exact-front"]])
        left = min([s.distance() for s in self.robot.range["front-left"]])
        right = min([s.distance() for s in self.robot.range["front-right"]])
        
        if self.__moveDirectlyOrNot__(position):
#        if self.directMove == True:
            print "direct Move"
            return True
        else:
            if front < dist or left < dist or right < dist:
                return False
            else:
                return True  
 
   def __moveDirectlyOrNot__(self, position):
        radius = self.robot.radius * 1000
        center = Point(self.robot.x*1000, self.robot.y*1000)
        topleft = Point(center.x+radius, center.y+radius)
        topright = Point(center.x+radius, center.y-radius)
        bottomleft = Point(center.x-radius, center.y+radius)
        bottomright = Point(center.x-radius, center.y-radius)
        
#        if self.__moveOrNot__(dist)
        
        if not self.gps.globalMap.hitObstacleOrNot(topleft.x, topleft.y, 
                                               position.x, position.y) and \
           not self.gps.globalMap.hitObstacleOrNot(topright.x, topright.y, 
                                               position.x, position.y) and \
           not self.gps.globalMap.hitObstacleOrNot(bottomleft.x, bottomleft.y, 
                                               position.x, position.y) and \
           not self.gps.globalMap.hitObstacleOrNot(bottomright.x, bottomright.y, 
                                               position.x, position.y):
            return True
        else:
            return False        
 
   def __follow__(self, dist, position):
#        print "follower"
        #control the motion of robot
        front = min([s.distance() for s in self.robot.range["exact-front"]])
        left = min([s.distance() for s in self.robot.range["front-left"]])
        right = min([s.distance() for s in self.robot.range["front-right"]])

        print "front ", front
        print "left ", left
        print "right ", right
#        print "position ", position
#        print "dist ", dist
        print "self.offsetDist ", self.offsetDist

        # check to see if it is necessary to follow the wall
        currentPoint = Point(self.robot.x*1000, self.robot.y*1000)
        if position.dist(currentPoint) < 350:
            print "we are there in follow mode!!!"
            return "SENSE" 
        elif self.__moveDirectlyOrNot__(position):
            print "we can reach the goal in line"
            self.directMove = True
            return "MOVE"
        else:
            self.directMove = False
#            if front < dist+self.offsetDist:
            if front < dist+0.1:    
                 # turn based on the goal position
#                 robotPoint = Point(self.robot.x*1000, self.robot.y*1000)
#                 theta = robotPoint.slopeAngle(position)             
#                 robotTheta = self.robot.th
#                 if self.robot.th > 180:
#                     robotTheta = self.robot.th - 360
#                 else:
#                     robotTheta = self.robot.th
#                 if theta > 180:
#                     theta = theta - 360
#                 else:
#                     theta = theta
#                     
#                 if math.fabs(theta - robotTheta) > 180:
#                     if theta < 0 and robotTheta > 0:
#                         theta = 360 + theta
#                     elif theta > 0 and robotTheta < 0:
#                         robotTheta = 360 + robotTheta                    
#                 turningDir = (theta - robotTheta) / math.fabs(theta - robotTheta) 
#                 rotate = turningDir * 0.2                              
                 if left > right:
                     rotate = 0.2
                 else:
                     rotate = -0.2
                 self.move(0, rotate)
                 print "follow action ", (0, rotate) 
                 return "FOLLOW"
            elif (left < dist+self.offsetDist or right < dist+self.offsetDist): 
                 print "self.offsetDist ", self.offsetDist
                 if left < dist+self.offsetDist:
    #                 print "left < dist"
    #                     return (0.2, -0.1)
                     self.move(0.15, -0.1)
                     print "follow action ", (0.15, -0.1) 
                     return "FOLLOW" 
                 else:
    #                 print "right < dist"
    #                     return (0.2, 0.1)
                     print "follow action ", (0.15, 0.1)
                     self.move(0.15, 0.1)
                     return "FOLLOW"    
            else:
                 # follow the wall
                 # change the angle first
                 if left > right:
                     x2, y2 = self.__getHits__('range', 32)
                     x3, y3 = self.__getHits__('range', 33)
                 else:
                     x2, y2 = self.__getHits__('range', 6)
                     x3, y3 = self.__getHits__('range', 7)
                 
                 print  "x2, y2 ", (x2, y2)
                 print  "x3, y3 ", (x3, y3)
                 turnPower = self.__calculateTurnAngle__(x2, y2, x3, y3)
                 if turnPower <> 0:
                     print "follow action ", (0, turnPower) 
                     self.move(0, turnPower)
                 else:
                     # if lost the wall, the robot should be able to find it;
                     if left > 0.8 and right > 0.8 and front > 0.8:
                         print "lost the wall, the robot is in a wide area "
                         return "MOVE"
                     else:
                         print "follow action ", (0.15, 0)
                         self.move(0.15, 0.0)
                 self.offsetDist = -0.2
                 return "FOLLOW"
 
   def __move2Position__(self, position): 
       currentPoint = Point(self.robot.x*1000, self.robot.y*1000)
       self.offsetDist = 0.0;
#       print "currentPoint ", currentPoint
#       print "position ", position
       print "currentPoint.dist(position) ", currentPoint.dist(position)
       if position.dist(currentPoint) < 350:
           print "we are there!!!"
           return "SENSE"
       else:
           # calculate the angle needed to be turned
           theta = currentPoint.slopeAngle(position)
           robotTheta = self.robot.th
           if self.robot.th > 180:
               robotTheta = self.robot.th - 360
           else:
               robotTheta = self.robot.th
           if theta > 180:
               theta = theta - 360
           else:
               theta = theta
           # generate the action first
           action = ()
           if math.fabs(theta-robotTheta) > 6:
               print "theta ", theta
               print "robotTheta ", robotTheta
               print "(theta-robotTheta) ", (theta - robotTheta)
               
               if math.fabs(theta - robotTheta) > 180:
                   if theta < 0 and robotTheta > 0:
                       theta = 360 + theta
                   elif theta > 0 and robotTheta < 0:
                       robotTheta = 360 + robotTheta                    
               turningDir = (theta - robotTheta) / math.fabs(theta - robotTheta)
               
               if math.fabs(theta - robotTheta) > 15:
                   turnPower = turningDir * 0.2
               elif math.fabs(theta - robotTheta) > 9:
                   turnPower = turningDir * 0.15
               else:
                   turnPower = turningDir * 0.05    
                                
               print "turnPower ", turnPower
               action = (0, turnPower)         
               self.move(0, turnPower)
               return "MOVE"          
           else:
               # then move straightly
               # first check to see if we can move
               if self.__moveOrNot__(0.6, position):
                   action = (0.2, 0)
                   self.move(0.2, 0)
                   print "keep moving ", (0.2, 0)
                   return "MOVE"
               else:
                   print "to follow"
                   return "FOLLOW"
#               self.move(0.2, 0)
  
   def __hitsCount__(self, pos):        
        item = 'range'
        originalUnits = self.robot.__dict__[item].units
        self.robot.__dict__[item].units = 'METERS'
        count = 0
        pos.dir = math.atan(float(self.robot.y-pos.y)/float(self.robot.x-pos.x)) 
        for i in range(self.robot.__dict__[item].count):
            # in MM:
            offx, offy, z, theta, arc = self.robot.__dict__[item][i].geometry
            # get start point and end point
            endX = math.cos(theta+pos.dir) * 8000 + pos.x
            endY = math.sin(theta+pos.dir) * 8000 + pos.y
            # check if the line hits obstacles
            if self.gps.globalMap.hitObstacleOrNot(pos.x, pos.y, endX, endY):
                count += 1
        # restore the unit
        self.robot.__dict__[item].units = originalUnits
        return count
  
   def __verifyFreeEdges__(self):
       print "****** *******"
       print "self.globalFreeEdges.__len__() ", self.globalFreeEdges.__len__()
       print "self.globalFreeEdges ", self.globalFreeEdges
       print "****** *******"
       
       indexList = []
       index = 0 
       for freeedge in self.globalFreeEdges: 
           midPoint = Point((freeedge[0][0]+freeedge[1][0])/2.0, (freeedge[0][1]+freeedge[1][1])/2.0)
           count = self.__hitsCount__(midPoint)
           print "count ", count
           # it hits so many obstacles, then this is not the free edge
           if count > 30:
               print "index needed to be deleted ", index
               indexList.append(index)
                     
#           if not self.gps.globalMap.isFreeEdge(freeedge[0][0], freeedge[0][1], \
#                                               freeedge[1][0], freeedge[1][1],):
#               print "freeedge deleted ", freeedge
#               indexList.append(index)
           index += 1
       # delete the free edges which are fake
       print "indexList.size ", indexList.__len__() 
       offset = 0
       for i in range(len(indexList)):
           print "indexList[i] ", indexList[i]
           del self.globalFreeEdges[indexList[i]+offset]
           offset -= 1
 
#       for index in indexList:
#           del self.globalFreeEdges[index]
  
   def __getFreeEdges__(self, obstacles):            
       freeEdges = []
       (i1, j1) = obstacles[0]
       for (i2,j2) in obstacles[1:]:
           dist = math.sqrt((i1-i2)**2+(j1-j2)**2)
           if dist > 1000: 
               list = [(i1, j1), (i2, j2)]
               freeEdges.append(list)
           i1 = i2
           j1 = j2
       return freeEdges
 
   def __sense__(self):
       # efficiency!!!
       if self.bFreeFont == False:
#           print "font free"
           self.freeEdgesFont = self.__getFreeEdges__(self.lps.obstacleGlobalPos)
#           print "self.freeEdgesFont ", self.freeEdgesFont
           self.bFreeFont = True
       if self.bFreeBack == False:
           # turn around first
           # change the angle range
           robotTheta = 0.0
           if self.robot.th > 180:
               robotTheta = self.robot.th - 360
           else:
               robotTheta = self.robot.th
           
           if self.robotPrevTheta == None:
#               print "set prev theta"
               if self.robot.th > 180:
                   self.robotPrevTheta = self.robot.th - 360
               else:
                   self.robotPrevTheta = self.robot.th
           # handle the delay 
#           print "self.robotPrevTheta ", self.robotPrevTheta
#           print "self.robot.th ", self.robot.th
           if math.fabs(robotTheta-self.robotPrevTheta) < 130:
#               print "turn around"
               self.move(0, 0.3)
               return "SENSE"
           else:
               self.freeEdgesBack = self.__getFreeEdges__(self.lps.obstacleGlobalPos)
#               print "self.freeEdgesBack ", self.freeEdgesBack
               
           self.bFreeBack = True
#               print "free back"
#               print self.freeEdgesBack
       # now the robot senses both font and back, let's combine the font and back edges
       freeEdges = self.freeEdgesBack + self.freeEdgesFont
       # combine it to the whole free edges list
       self.globalFreeEdges = self.globalFreeEdges + freeEdges
       # verify the free edges to see if they are real free edges
       self.__verifyFreeEdges__()
       # reset the variables
       self.freeEdgesBack = []
       self.freeEdgesFont = []
       self.bFreeBack = False
       self.bFreeFont = False
       self.robotPrevTheta = None
       
       print "combine free edges"
       print self.globalFreeEdges
       print "*******************************************" 
       # change to the next state
       return "NEXT"

   def __deleteFreeEdge__(self, nextPosition):
       # find the free edge
       index = 0
       minDist = 1000000
       minIndex = 0
       for freeedge in self.globalFreeEdges:         
            midPoint = Point((freeedge[0][0]+freeedge[1][0])/2, (freeedge[0][1]+freeedge[1][1])/2)
            dist = midPoint.dist(nextPosition)
            if dist < minDist:
                minDist = dist
                minIndex = index
            index += 1
       # delete that free edge
       print "delete index ", minIndex
       del self.globalFreeEdges[minIndex]            
         
   def step(self):
      #print "Stepping...",
      # First we clear out all our old LPS data
      self.lps.reset()
      # Next we update our LPS with current 'range' sensor readings
      self.lps.sensorHits(self.robot, 'range')
      # Now redraw our LPS window - the LPS redraw can be improve performance
      self.gps.updateFromLPS(self.lps, self.robot)
      self.need_redraw = True
      
      # the loop
      if self.status == "SENSE":
           # do some sensing work
#           print "sense"    
           self.status = self.__sense__()
#           self.status = "SENSE"
      elif self.status == "NEXT":
           print "calculating best view with PSO"                     
           # use PSO 
           psoAlgorithm = PSO(10, 30, self.robot, self.gps.globalMap, self.globalFreeEdges, 350)
#           self.nextBestView = psoAlgorithm.getBestParticle()
           self.nextBestView = psoAlgorithm.getNextBestView(self.globalFreeEdges)
#           self.__deleteFreeEdge__(self.nextBestView)
           
#           self.nextBestView = Point(1850,-1169)
#           self.nextBestView = Point(400, -3000)
#           self.nextBestView = Point(1833, -1272)
#           self.nextBestView = self.positionList[self.posIndex]
#           self.posIndex += 1
           print "self.nextBestView ", self.nextBestView
#           print "self.robot.x*1000 ", self.robot.x*1000
#           print "self.robot.y*1000 ", self.robot.y*1000 
           
           self.status = "MOVE"
      elif self.status == "MOVE":
           # move and check if the robot is there
           print "robot move"
           self.status = self.__move2Position__(self.nextBestView)
      elif self.status == "FOLLOW":
           self.status = self.__follow__(0.6, self.nextBestView)
           
#           if not self.__move2Position__(self.nextBestView):
#               # if the robot is there
#               print "robot changes state"
#               self.status = "SENSE"
#      elif self.status == "FOLLOW":
#          self.status = self.__follow__(0.6);
      
   def redraw(self):
      #print "Redrawing...",
      if self.need_redraw:
         self.lps.redraw(drawLabels=False)
         self.gps.update()
#         self.gps.redraw(self.nextBestView)
         self.gps.redraw()
         self.need_redraw = False
      #print "done redrawing!"
       
   def destroy(self):
      # Make sure we close down cleanly
      self.lps.destroy()
      self.gps.destroy()
      #self.joystick.destroy()
       
       
def INIT(engine):
   return MapBuilder("Mapping Brain", engine)
